//
//  main.c
//  OpenCV_CaptureCam
//
//  Created by MinHo Park on 13. 1. 3..
//  Copyright (c) 2013년 MinHo Park. All rights reserved.
//

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <memory.h>
#include <pthread.h>
#include <cv.h>
#include <opencv/highgui.h>
//#include "HarrisCorner.h"

#include "/Users/Maverick/Project/VC_Codec_Libs/Proj_VPx/Kakao_VCLib_VPx_Export/Kakao_VCLib_VPx_CodecExport.h"

#define Thread          signed int
#define TSK_YIELD()     usleep(1)        //Sleep(0)      // take care of YIELD
#define TSK_SLEEP(n)    usleep(n)
#define CREATE_THREAD   CreateThread
#define RESUME_THREAD   ResumeThread
#define EXIT_THREAD     GetExitCodeThread
#define SUSPEND_THREAD  SuspendThread

//#define __USE_LOCK_OBJ__

#ifdef __USE_LOCK_OBJ__
#define CS              CRITICAL_SECTION
#define InitCS(a)       InitializeCriticalSection(&(a))
#define EnterCS(a)      EnterCriticalSection(&(a))
#define ExitCS(a)       LeaveCriticalSection(&(a))
#define UnInitCS(a)     DeleteCriticalSection(&(a))
#else
#define CS              pthread_mutex_t
#define InitCS(a)       pthread_mutex_init(&(a), NULL)
#define EnterCS(a)      pthread_mutex_lock(&(a))    // 100 : time out
#define ExitCS(a)       pthread_mutex_unlock(&(a))
#define UnInitCS(a)     pthread_mutex_destroy(&(a))
#endif

#define MinPixel        (0)
#define MaxPixel        (255)
#define Clip3(nMin, nMax, x)         x > nMax ? nMax : (x < nMin ? nMin : x)


#define      MAX_FRAME_BUF_NUM      10

CvCapture	*capture = NULL;
IplImage	*frame[MAX_FRAME_BUF_NUM] = {NULL};
int			fps = 0;
int			nWidth = 0;
int			nHeight = 0;
int			nFrameWriteIndex = 0;
int			nFrameReadIndex = 0;


CS	nCriticalSection[MAX_FRAME_BUF_NUM];

void view_hist(IplImage *simg, IplImage *hist_img, int num, char *view_name)
{
	CvHistogram		*hist;
	float 			max_value = 0;
	float 			hist_val = 0;
	int 			intensity = 0;
	int 			axis_point = nHeight - 10;
	int 			hist_size[] = {255};
	IplImage		*plane_fir = cvCreateImage( cvGetSize(simg), 8, 1);
	IplImage		*planes[] = {plane_fir};
	CvFont 			*font;
	char 			s_text[512];
	int				i;
	float			h_ranges[]  = { 0, 255 };
	float			s_ranges[]  = { 0, 255 };
	float			*ranges[]  = { h_ranges, s_ranges };
    
	font = (CvFont *) malloc(sizeof(CvFont));
    
	cvInitFont(font, CV_FONT_VECTOR0, 0.5f, 0.5f, 0, 1, 1);
	
	hist = cvCreateHist(1, hist_size, CV_HIST_UNIFORM, ranges, 1);
	
	switch(num)
	{
		case 0 :
			cvCvtPixToPlane(simg, plane_fir,NULL, NULL, 0);
			cvZero(hist_img);
			break;
		case 1 :
			cvCvtPixToPlane(simg, NULL, plane_fir, NULL, 0);
			break;
		case 2 :
			cvCvtPixToPlane(simg, NULL, NULL, plane_fir, 0);
			break;
	}
    
	cvCalcHist(planes, hist, 0, 0);
	cvGetMinMaxHistValue(hist, 0, &max_value, 0, 0);
	
	for(i = 0; i < 255; i++)
	{
		int		nTmpXPos = 10 + i * (int)((float)(nWidth-10)/(float)256.0) + num;
        
		hist_val  = cvQueryHistValue_1D(hist,i);
		intensity = cvRound( hist_val*(float)(nHeight - 10)/(float)max_value);
        
		cvLine(hist_img, cvPoint(nTmpXPos, axis_point), cvPoint(nTmpXPos, abs(axis_point - intensity) ),
               num==0 ? CV_RGB(255,0,0) : num==1 ? CV_RGB(0,255,0) : num==2 ? CV_RGB(0,0,255) : CV_RGB(0,0,255), 1, 1, 0);
        
		if(0)//if ( (i % 20) == 0 )
		{
			sprintf( s_text, "%d", i);
			cvPutText( hist_img, s_text, cvPoint(i+20, axis_point+10), font, CV_RGB(255,0,0));
		}
	}
    
	// histogram title
	//************************************************************************************
	//cvPutText(hist_img, "-Channel Histogram", cvPoint(80, axis_point+25), font, CV_RGB(255,255,255));
	//cvPutText(hist_img, view_name, cvPoint(70, axis_point+25), font, CV_RGB(255,255,255));
}


void ColorConvert_RGBtoYUV420_App(IplImage *pSrc, VPxImgInfo *pVPxImgInfo)
{
    int            i, j;
    int            nWidth = pSrc->width;
    int            nHeight = pSrc->height;
    int            nPixPos;
    
    unsigned char      *pSrcRGB = (unsigned char *)(pSrc->imageData);
    unsigned char      *pDstY = pVPxImgInfo->pPlane[PLANE_Y];
    unsigned char      *pDstU = pVPxImgInfo->pPlane[PLANE_U];
    unsigned char      *pDstV = pVPxImgInfo->pPlane[PLANE_V];
    
    cvFlip(pSrc, NULL, 0);
    
    for(j=0 ; j<nHeight ; j++)
    {
        for(i=0 ; i<nWidth*3 ; i+=3)
        {
            nPixPos = (j * nWidth * 3) + i;
            *(pDstY++) = Clip3(MinPixel, MaxPixel, (int)( (0.257 * (double)pSrcRGB[nPixPos+2]) + (0.504 * (double)pSrcRGB[nPixPos+1]) + (0.098 * (double)pSrcRGB[nPixPos]) + 16));
            
            if((((i/3) & 0x01) == 0) && ((j & 0x01) == 0))
            {
                *(pDstU++) = Clip3(MinPixel, MaxPixel, (int)(-(0.148 * (double)pSrcRGB[nPixPos+2]) - (0.291 * (double)pSrcRGB[nPixPos+1]) + (0.439 * (double)pSrcRGB[nPixPos]) + 128));
                *(pDstV++) = Clip3(MinPixel, MaxPixel, (int)(  (0.439 * (double)pSrcRGB[nPixPos+2]) - (0.368 * (double)pSrcRGB[nPixPos+1]) - (0.071 * (double)pSrcRGB[nPixPos]) + 128));
            }
        }
        
        //pDstY += nPaddingSize - nWidth;
        
        if(((j+1) & 0x01) == 0)
        {
            //pDstU += nPaddingSize - (nWidth >> 1);
            //pDstV += nPaddingSize - (nWidth >> 1);
        }
    }
    
    cvFlip(pSrc, NULL, 0);
}



void ColorConvert_YUV420toRGB_App(VPxImgInfo *pVPxImgInfo, IplImage *pDst)
{
    int            i, j, ii;
    int            nWidth = pDst->width;
    int            nHeight = pDst->height;
    int            nPixPos;
    
    unsigned char      *pSrcY = pVPxImgInfo->pPlane[PLANE_Y];
    unsigned char      *pSrcU = pVPxImgInfo->pPlane[PLANE_U];
    unsigned char      *pSrcV = pVPxImgInfo->pPlane[PLANE_V];
    unsigned char      *pDstRGB = (unsigned char *)(pDst->imageData);
    
    
    for(j=0 ; j<nHeight ; j++)
    {
        for(i=0 ; i<nWidth ; i++)
        {
            ii = i * 3;
            nPixPos = ((nHeight - j - 1) * nWidth * 3) + ii;
            
            pDstRGB[nPixPos+2]     = Clip3(MinPixel, MaxPixel, (int)(1.164*(double)(*(pSrcY) - 16) + 1.596*(double)(*(pSrcV) - 128)));               // R
            pDstRGB[nPixPos+1] = Clip3(MinPixel, MaxPixel, (int)(1.164*(double)(*(pSrcY) - 16) - 0.391*(double)(*(pSrcU) - 128) - 0.813*(double)(*(pSrcV) - 128)));
            pDstRGB[nPixPos] = Clip3(MinPixel, MaxPixel, (int)(1.164*(double)(*(pSrcY) - 16) + 2.018*(double)(*(pSrcU) - 128)));              // B
            
            pSrcY++;
            if((i & 0x01) == 1)
            {
                pSrcU++;
                pSrcV++;
            }
        }
        
        pSrcY += pVPxImgInfo->nStride[PLANE_Y] - nWidth;
        
        if((j & 0x01) == 0)
        {
            pSrcU -= (nWidth >> 1);
            pSrcV -= (nWidth >> 1);
        }
        else
        {
            pSrcU += pVPxImgInfo->nStride[PLANE_U] - (nWidth >> 1);
            pSrcV += pVPxImgInfo->nStride[PLANE_V] - (nWidth >> 1);
        }
    }  
}


void Thread_Function_GrapImg(void *pData)
{
	int			key = 0;
	int			nCount = 0;
	CvFont		*font;
    
	/* display video */
    //cvNamedWindow("Video_Hist", 0);
    
	font = (CvFont *) malloc(sizeof(CvFont));
    
	cvInitFont(font, CV_FONT_VECTOR0, 0.5f, 0.5f, 0, 1, 1);
    
    //cvResizeWindow("Video_Hist", nWidth, nHeight);
    
    while(key != 'q')
	{
        /* get a frame */
        if(NULL == frame[nFrameWriteIndex])
        {
			EnterCS(nCriticalSection[nFrameWriteIndex]);
	        if(NULL == (frame[nFrameWriteIndex] = cvQueryFrame(capture)))
				break;
            
			//sprintf(text, "%d", nFrameNum++);
			//cvPutText(frame[nFrameWriteIndex], text, cvPoint(50, 50), font, CV_RGB(255, 0, 0));
            ExitCS(nCriticalSection[nFrameWriteIndex]);

            //printf("    %s - WriteIndex: %d\n", __FUNCTION__, nFrameWriteIndex);

			nFrameWriteIndex++;
			nFrameWriteIndex %= MAX_FRAME_BUF_NUM;
			nCount = 0;
		}
		else
		{
			//cvWaitKey( 1000 / fps );
			//printf("	%s - Sleep\n", __FUNCTION__);
            
			if(nCount++ > 30)
				break;
            
			TSK_SLEEP(1000 * fps);
			continue;
		}
    }
    
	//cvDestroyWindow("Video_Hist");
	
	printf("    %s - Exit\n", __FUNCTION__);
    
    return;
}


#define ENC_TYPE                    "VP8"
#define ENC_IMG_WIDTH               (640)
#define ENC_IMG_HEIGHT              (360)
#define IMG_SCALE_FACTOR            (1.0f)
#define ENC_TARGETBITRATE           (1000)
#define ENC_FRAME_RATE              (30.0f)
#define ENC_GOP_SIZE                (30)
#define ENC_ENABLE_CBR_MODE         (1)


extern int      *_g_TargetBitrate;


void Thread_Function_ShowImg(void *pData)
{
	void                        *pEncHandle = NULL;	// Encoder main handle
	void                        *pDecHandle = NULL;	// Encoder main handle
	IplImage                    *pEncodingResizeImg = NULL;
	IplImage                    *pDecodedImg = NULL;
	CvSize                      nResizedImgSize;
    VPxImgInfo                  EncImgInfo = {0, };
    VPxImgInfo                  DecImgInfo = {0, };
    unsigned char               *pEncodedBuf = NULL;
	unsigned char               *pInPlane[] = {NULL, NULL, NULL};
    unsigned char               *pOutPlane[] = {NULL, NULL, NULL};
    unsigned int                key = 0, nCount = 0, i = 0;
    unsigned int                nEncodedStreamLen = 0;
    unsigned int                nIsKeyFrame = 0;
    unsigned int                nProcessedFrameCnt = 0;
	unsigned int                iYSize = nWidth * nHeight;
	unsigned int                iUSize = nWidth * nHeight / 4;
	unsigned int                iVSize = iUSize;
    unsigned int                nBitrateArr[(unsigned int)ENC_FRAME_RATE] = {0, };
    unsigned int                nAvgBitrate = 0;
    unsigned int                MinBR = 0xFFFFFFFF, MaxBR = 0;
    signed int                  nTargetBR = ENC_TARGETBITRATE;
    CvFont                      *pFont = NULL;
	char                        text[200] = {0};
    

   	pFont = (CvFont *) malloc(sizeof(CvFont));
    
	cvInitFont(pFont, CV_FONT_VECTOR0, 0.35f, 0.35f, 0, 1, 1);

	nResizedImgSize.width = ((float)ENC_IMG_WIDTH * IMG_SCALE_FACTOR);
	nResizedImgSize.height = ((float)ENC_IMG_HEIGHT * IMG_SCALE_FACTOR);
	pEncodingResizeImg = cvCreateImage(nResizedImgSize, IPL_DEPTH_8U, 3);
	pDecodedImg = cvCreateImage(nResizedImgSize, IPL_DEPTH_8U, 3);

	// Allocating buffers
	pInPlane[0] = malloc(iYSize);
	pInPlane[1] = malloc(iUSize);
	pInPlane[2] = malloc(iVSize);
    EncImgInfo.nWidth = nResizedImgSize.width;
    EncImgInfo.nHeight = nHeight;
    for(i=0 ; i<3 ; i++)
    {
        EncImgInfo.pPlane[i] = pInPlane[i];
        EncImgInfo.nStride[i] = (0==i) ? nResizedImgSize.width : (nWidth>>1);
    }
    
	pOutPlane[0] = malloc(iYSize);
	pOutPlane[1] = malloc(iUSize);
	pOutPlane[2] = malloc(iVSize);
    DecImgInfo.nWidth = nWidth;
    DecImgInfo.nHeight = nHeight;
    for(i=0 ; i<3 ; i++)
    {
        DecImgInfo.pPlane[i] = pOutPlane[i];
        DecImgInfo.nStride[i] = (0==i) ? nResizedImgSize.width : (nWidth>>1);
    }
    
    pEncodedBuf = malloc(1000 * 500);
    
	// Create & Initialize Encoder
	Kakao_VPxEnc_Create(&pEncHandle);
	Kakao_VPxEnc_Init(pEncHandle, nResizedImgSize.width, nResizedImgSize.height, ENC_TARGETBITRATE, ENC_FRAME_RATE, ENC_GOP_SIZE, ENC_GOP_SIZE);
    
	Kakao_VPxDec_Create(&pDecHandle);
	Kakao_VPxDec_Init(pDecHandle);

    cvResizeWindow("Original_Img", nResizedImgSize.width, nResizedImgSize.height);
    cvResizeWindow("Decoded_Img", nResizedImgSize.width, nResizedImgSize.height);
    
    cvCreateTrackbar("TargetBitrate", "Decoded_Img", &nTargetBR, 3000, 0);
    
	while(key != 'q')
	{
		/* always check */
		if(NULL != frame[nFrameReadIndex])
		{
			//printf("    %s - ReadIndex: %d\n", __FUNCTION__, nFrameReadIndex);
            
			/* display frame */
			EnterCS(nCriticalSection[nFrameReadIndex]);
            cvResize(frame[nFrameReadIndex], pEncodingResizeImg, CV_INTER_LINEAR);
            //cvCvtColor(pEncodingResizeImg, pEncodingYUVImg, CV_BGR2YUV_I420);

            ColorConvert_RGBtoYUV420_App(pEncodingResizeImg, &EncImgInfo);
            Kakao_VPxEnc_Encode(pEncHandle, &(EncImgInfo), nProcessedFrameCnt++, 0, &nEncodedStreamLen, pEncodedBuf, &nIsKeyFrame);
            
            // Calculate Statistics
            {
                nAvgBitrate = 0;
                nBitrateArr[nProcessedFrameCnt%((unsigned int)ENC_FRAME_RATE)] = nEncodedStreamLen;
                for(i=0 ; i<((unsigned int)ENC_FRAME_RATE) ; i++)
                    nAvgBitrate += nBitrateArr[i];
                nAvgBitrate = (nAvgBitrate*8/1000);
                
                MinBR = MIN(MinBR, nEncodedStreamLen);
                MaxBR = MAX(MaxBR, nEncodedStreamLen);
            }
            
            Kakao_VPxDec_Decode(pDecHandle, pEncodedBuf, nEncodedStreamLen, &(DecImgInfo));
            ColorConvert_YUV420toRGB_App(&DecImgInfo, pDecodedImg);
            
            *_g_TargetBitrate = nTargetBR * 1000;

            sprintf(text, "CodecType: %s", ENC_TYPE);
			cvPutText(pDecodedImg, text, cvPoint(20, 50), pFont, CV_RGB(255, 255, 255));
            sprintf(text, "TargetBR: %dKbps", nTargetBR);
			cvPutText(pDecodedImg, text, cvPoint(20, 70), pFont, CV_RGB(255, 255, 255));
            sprintf(text, "AverageVR: %04dKbps   CurrBR: %04dKbps", nAvgBitrate, (nEncodedStreamLen*8/1000));
            cvPutText(pDecodedImg, text, cvPoint(20, 90), pFont, CV_RGB(255, 255, 255));
            sprintf(text, "Min(%03d) /  Max(%03d)", MinBR, MaxBR);
			cvPutText(pDecodedImg, text, cvPoint(20, 110), pFont, CV_RGB(255, 255, 255));
            sprintf(text, "fps: %.02f", ENC_FRAME_RATE);
			cvPutText(pDecodedImg, text, cvPoint(20, 130), pFont, CV_RGB(255, 255, 255));

			cvShowImage("Original_Img", pEncodingResizeImg);
			cvShowImage("Decoded_Img", pDecodedImg);
            
		    nCount = 0;
		    ExitCS(nCriticalSection[nFrameReadIndex]);
            
			/* quit if user press 'q' */
			if('q' == (key = cvWaitKey(1)))
                break;
            
			EnterCS(nCriticalSection[nFrameReadIndex]);
			frame[nFrameReadIndex] = NULL;
			ExitCS(nCriticalSection[nFrameReadIndex]);
            
            nFrameReadIndex++;
			nFrameReadIndex %= MAX_FRAME_BUF_NUM;
		}
		else
		{
			//printf("    %s - Sleep\n", __FUNCTION__);
            
			if(nCount++ > 30)
				break;
            
			TSK_SLEEP(1000 * fps);
			continue;
		}
	}
    
    Kakao_VPxEnc_Close(pEncHandle);
	Kakao_VPxDec_Close(pDecHandle);
    
	// Release all buffers
    free(pEncodedBuf);
	free(pInPlane[0]);
	free(pInPlane[1]);
	free(pInPlane[2]);
	free(pOutPlane[0]);
	free(pOutPlane[1]);
	free(pOutPlane[2]);
    
	printf("    %s - Exit\n", __FUNCTION__);
    
    return;
}


#define GRAP_IMAGE_THREAD      0
#define SHOW_IMAGE_THREAD      1

pthread_t threads[5];

int main(void)
{
    unsigned int    i = 0;
    
    /* load the AVI file */
    capture = cvCaptureFromCAM(0);
    
    /* always check */
    if(!capture)
		return 0;
    
    /* get fps, needed to set the delay */
    fps = 30;//(int)cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);
    nWidth = ((int)cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH)/2);
    nHeight = ((int)cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT)/2);
    
    cvNamedWindow("Original_Img", 0);
    cvNamedWindow("Decoded_Img", 0);
    
    for(i=0 ; i<MAX_FRAME_BUF_NUM ; i++)
        InitCS((nCriticalSection[i]));
    
    pthread_create(&threads[GRAP_IMAGE_THREAD], NULL, (void *)(&Thread_Function_GrapImg), NULL);
    if(threads[GRAP_IMAGE_THREAD] == NULL)
        printf("Thread Creation Error\n");

#if 0
    pthread_create(&threads[SHOW_IMAGE_THREAD], NULL, (void *)(&Thread_Function_ShowImg), NULL);
    if(threads[SHOW_IMAGE_THREAD] == NULL)
        printf("Thread Creation Error\n");
#else
    Thread_Function_ShowImg((void *)1);
#endif
    
    pthread_join(threads[GRAP_IMAGE_THREAD], NULL);
    pthread_join(threads[SHOW_IMAGE_THREAD], NULL);
    
    cvDestroyWindow("Original_Img");
    cvDestroyWindow("Decoded_Img");

    pthread_detach(threads[GRAP_IMAGE_THREAD]);
    pthread_detach(threads[SHOW_IMAGE_THREAD]);

    for(i=0 ; i<MAX_FRAME_BUF_NUM ; i++)
        UnInitCS((nCriticalSection[i]));
    
    
	/* free memory */
    cvReleaseCapture(&capture);
    
    return 0;
}

